package simulacao;

import lejos.nxt.*;
import lejos.nxt.comm.*;
import java.io.*;

public class Servidor {
    final public static int NORTE=1;
    final public static int LESTE=2;
    final public static int SUL=3;
    final public static int OESTE=4;

    public static int offSet; //contém a rotação atual do robo em relação a origem (0 a 359)
    public static boolean radarNorte;
    public static boolean radarSul;
    public static boolean radarLeste;
    public static boolean radarOeste;

    public static final int GR_POR_PASSO=620;
    public static final int GR_POR_GIRO=180;
    public static final int VL_RADAR=400;
    public static final int VL_ANDA=400;
    public static final int VL_GIRA=250;

    public static int radarNorteVal;
    public static int radarSulVal;
    public static int radarLesteVal;
    public static int radarOesteVal;

    public static int posAtual = 0;
    public static UltrasonicSensor sonic;
    public static int limiarSensor = 17; //ajustar de acordo com o tamanho da maquete;

    public static char frentePara='N';
    public static Bussola B;
    public static void main(String[] args) throws Exception {
        sonic = new UltrasonicSensor(SensorPort.S1);
        B  = new Bussola(NORTE);


        Motor.A.setSpeed(VL_RADAR);

        Motor.B.setSpeed(VL_ANDA);
        Motor.B.smoothAcceleration(true);
        Motor.C.setSpeed(VL_ANDA);
        Motor.C.smoothAcceleration(true);
        //Motor.A.rotate(90);
        //runRadar();
        showStatus();
        char c;
        boolean cy = true;
        BTConnection btc = Bluetooth.waitForConnection();
        DataInputStream in = btc.openDataInputStream();
        DataOutputStream out = btc.openDataOutputStream();
        while (cy) {
            //DataOutputStream out = btc.openDataOutputStream();
            /*Protocolo
             *
             * I: Info
             * F: Fim
             * S: Sul
             * N: Norte
             * L: Leste
             * O: Oeste
             */

            c = in.readChar();
            if (c == 'F') {
                cy = false;
            }
            if (c == 'S') {
                //status
                MoveTo(SUL);
            }
            if (c == 'N') {
                MoveTo(NORTE);
            }
            if (c == 'L') {
                MoveTo(LESTE);
            }
            if (c == 'O') {
                MoveTo(OESTE);
            }
            if (c == 'E') {
                viraEsquerda();
            }
            if (c == 'D') {
                viraDireita();
            }
            if (c == 'A') {
                Anda();
            }
            runRadar();
            //runRadar();
            // enviar a leitura do sensor
            out.writeBoolean(radarNorte);
            out.writeBoolean(radarSul);
            out.writeBoolean(radarLeste);
            out.writeBoolean(radarOeste);


            //
            out.flush();
            //out.close();
            Thread.sleep(100); // wait for data to drain
            showStatus();
        }
        out.close();
        in.close();
        btc.close();
        if (posAtual ==270) Motor.A.rotate(-270);
    }

    public static void MoveTo(int Direcao){
        switch (Direcao){
            case NORTE:
                switch (B.ponteiro){
                    case NORTE:
                        Anda();
                        break;
                    case SUL:
                        viraDireita();
                        viraDireita();
                        Anda();
                        break;
                    case LESTE:
                        viraEsquerda();
                        Anda();
                        break;
                    case OESTE:
                        viraDireita();
                        Anda();
                        break;
                }
                break;
            case SUL:
                switch (B.ponteiro){
                    case SUL:
                        Anda();
                        break;
                    case NORTE:
                        viraDireita();
                        viraDireita();
                        Anda();
                        break;
                    case OESTE:
                        viraEsquerda();
                        Anda();
                        break;
                    case LESTE:
                        viraDireita();
                        Anda();
                        break;
                }
                break;
            case LESTE:
                switch (B.ponteiro){
                    case LESTE:
                        Anda();
                        break;
                    case OESTE:
                        viraDireita();
                        viraDireita();
                        Anda();
                        break;
                    case SUL:
                        viraEsquerda();
                        Anda();
                        break;
                    case NORTE:
                        viraDireita();
                        Anda();
                        break;
                }
                break;
            case OESTE:
                switch (B.ponteiro){
                    case OESTE:
                        Anda();
                        break;
                    case LESTE:
                        viraDireita();
                        viraDireita();
                        Anda();
                        break;
                    case NORTE:
                        viraEsquerda();
                        Anda();
                        break;
                    case SUL:
                        viraDireita();
                        Anda();
                        break;
                }
                break;
        }



    }

    public static void viraDireita() {
        Motor.B.setSpeed(VL_GIRA);
        Motor.C.setSpeed(VL_GIRA);
        Motor.B.rotate(GR_POR_GIRO * -1, true);
        Motor.C.rotate(GR_POR_GIRO, true);
        while (Motor.C.isMoving());
        B.Direita();
    }

    public static void viraEsquerda() {
        Motor.B.setSpeed(VL_GIRA);
        Motor.C.setSpeed(VL_GIRA);
        Motor.B.rotate(GR_POR_GIRO, true);
        Motor.C.rotate(GR_POR_GIRO * -1, true);
        while (Motor.C.isMoving());
        B.Esquerda();
    }

    public static void Anda() {
        //faz o robo caminhar uma distância x a frente
        int x = GR_POR_PASSO;
        Motor.B.setSpeed(VL_ANDA);
        Motor.C.setSpeed(VL_ANDA);
        Motor.B.rotate(x, true);
        Motor.C.rotate(x, true);
        while (Motor.C.isMoving());
    }

    public static boolean getSensor() throws InterruptedException {
        sonic.ping();
        Thread.sleep(100);
        if (sonic.getDistance() <= limiarSensor) {
            return true;
        } else {
            return false;
        }
    }

    public static int getSensorVal() throws InterruptedException {
        sonic.ping();
        Thread.sleep(100);
        return sonic.getDistance();
    }

    public static void showStatus() {
        LCD.clear();
        String mensa;
        mensa = "Sensor Leste: " + Integer.toString(radarLesteVal);
        LCD.drawString(mensa, 0, 0);
        mensa = "Sensor Oeste: " +  Integer.toString(radarOesteVal);
        LCD.drawString(mensa, 0, 1);
        mensa = "Sensor Norte: " +  Integer.toString(radarNorteVal);
        LCD.drawString(mensa, 0, 2);
        mensa = "Sensor Sul  : " +  Integer.toString(radarSulVal);
        LCD.drawString(mensa, 0, 3);
        mensa = "Tacometro A : " + Motor.A.getTachoCount();
        LCD.drawString(mensa, 0, 4);
        LCD.refresh();
    }

    public static void runRadar() throws InterruptedException {
        /* método que realiza a leitura do radar
         * O sensor deve rodar 270 graus. A cada 90
         * graus uma região é analisada.
         */
        //leitura pos 1

        switch (B.ponteiro){
            case NORTE:
                radarNorte = getSensor();
                radarNorteVal = getSensorVal();
                Motor.A.rotate(90);
                radarOeste = getSensor();
                radarOesteVal = getSensorVal();
                Motor.A.rotate(90);
                radarSul = getSensor();
                radarSulVal = getSensorVal();
                Motor.A.rotate(90);
                radarLeste = getSensor();
                radarLesteVal = getSensorVal();
                break;
            case SUL:
                radarSul = getSensor();
                radarSulVal = getSensorVal();
                Motor.A.rotate(90);
                radarLeste = getSensor();
                radarLesteVal = getSensorVal();
                Motor.A.rotate(90);
                radarNorte = getSensor();
                radarNorteVal = getSensorVal();
                Motor.A.rotate(90);
                radarOeste = getSensor();
                radarOesteVal = getSensorVal();
                break;
            case LESTE:
                radarLeste = getSensor();
                radarLesteVal = getSensorVal();
                Motor.A.rotate(90);
                radarNorte = getSensor();
                radarNorteVal = getSensorVal();
                Motor.A.rotate(90);
                radarOeste = getSensor();
                radarOesteVal = getSensorVal();
                Motor.A.rotate(90);
                radarSul = getSensor();
                radarSulVal = getSensorVal();
                break;
            case OESTE:
                radarOeste = getSensor();
                radarOesteVal = getSensorVal();
                Motor.A.rotate(90);
                radarSul = getSensor();
                radarSulVal = getSensorVal();
                Motor.A.rotate(90);
                radarLeste = getSensor();
                radarLesteVal = getSensorVal();
                Motor.A.rotate(90);
                radarNorte = getSensor();
                radarNorteVal = getSensorVal();
                break;
                            
        }

        Motor.A.rotate(-270);

    }



}
